#pragma once
#include <memory>
#include <vector>
#include <list>
#include "pfsm_state.h"
#include "zuc/nml_intf/zucpos2.h"

namespace axisgroup {
class Robot;
class PFsmServo : public PFsmState
{
public:
    PFsmServo(Robot* grp);

private:
    bool is_plan_over() override;
    bool is_stopping() override;
    int enter() override;
    void exit() override;
    int update_plan() override;
    void abort() override;
    void reset();
    bool wish_exit_ = false;
    bool last_wish_exit = false;

public:
    // 动力学辨识状态
    enum DynIdentifyStatus
    {
        DYN_STAND_BY = 0,
        DYN_IN_MOV = 1,
        DYN_MOV_OVER = 2,
        DYN_IN_CAL = 3,
        DYN_CAL_OVER = 4,
        DYN_IN_SET = 5,
        DYN_SET_OVER = 6

    } dynamics_identifying_status_ = DYN_STAND_BY;

private:
    struct DynIdentify
    {
        int traj_id_ = 0;
        int time_count_ = 0;  // 动力学辨识时的时间计数
        int one_cycle_num = 0;
        double a1[MAX_AXIS][4] = {};
        double a2[MAX_AXIS][4] = {};
        double a3[MAX_AXIS][4] = {};
        double b1[MAX_AXIS][4] = {};
        double b2[MAX_AXIS][4] = {};
        double b3[MAX_AXIS][4] = {};
        double c[MAX_AXIS][4] = {};
        double W1_ = 2.0;
        double W2_ = 8.0;
        double W3_ = 16.0;
    } dyn_idt_;

    void get_identify_traj_pos(RobJointVal& pos);
    int check_identify_time_count();
};
}  // namespace axisgroup
